//
//  world.m
//  SSE
//
//  Created by Matt Roman on 10/2/07.
//  Copyright 2007 OU IRL. All rights reserved.
//

#import "world.h"

@interface world (privatestuff)
// loads a new set of random obstacles
- (BOOL) RandomizeObstacles;
	// merges two Cspace objects and returns:
	//-1 - ERROR regenerate a new world
	// 0 - do not intersect
	// 1 - if object 1 needs to be removed
	// 2 - if object 2 needs to be removed
- (int) MergeCspace:(Cspace *)CS1 withCspace:(Cspace *)CS2;
	// prunes the configSpace array and combines all overlapping objects
	// returns -1 if it cannot because of an error
- (int) CreateCspace;
@end

@implementation world

- (id)initWorldWithGoal:(Point3f)g
{	
	[super init];
	numObstacles = NUMOBS;
	worldNumber = 0;
	goal = g;
	wind = 0.0;
	if(![self RandomizeObstacles])[self ResetObstacles];
	return self;
}

- (BOOL)RandomizeObstacles
{
	int i,err;
	float crateL,crateW,crateH;
	
	Crate *block;
	Cspace *outline;
		
	coverageArea = 0;
					
	obstacles = [[NSMutableArray alloc] init];
	configSpace = [[NSMutableArray alloc] init];

	for(i = 0; i < numObstacles; i++)	// create obstacles with random positions, angles, and sizes
	{
			while(1){
				crateL = 0.1+Randomn()*MAXL;
				crateW = 0.1+Randomn()*MAXW;
				crateH = 0.5+Randomn()*MAXH;
			
				// create a temp obstacle
				block = [[Crate alloc]initCrateAlpha:Randomn()*PI/2 
												   X:3.0 + WORLDX*Randomn()
												   Y:1.0 + WORLDY*Randomn()
												   Z:0.0
												   L:crateL
												   W:crateW
												   H:crateH];
				// set the color of the obstacle
				[block setColorR:0.8 G:0.1 B:0.1 A:1];
				// create the initial Cspace around each obstacle
				outline = [[Cspace alloc]initCspaceWithCrate:block ID:i];
			
				if([outline isPointInside:&goal]) // make sure no obstacles are placed over the goal
				{
					[block release];
					[outline release];
				}
				else break;
			}
		// save the obstacles and Cspace objects in the corresponding arrays
		[obstacles addObject:block];
		[configSpace addObject:outline];
		// release the temp memory spaces created
		[block release];
		[outline release];
		
		coverageArea += (crateL * crateW);
	}

	// continue doing passes over the Cspace to ensure all objects are merged
	do{
		i = [configSpace count];
		if((err = [self CreateCspace]) == -1) break;
	}while(i != [configSpace count]);
	// check again if the Cspace was able to merge if not restart
	if(err == -1){
		return NO;
	}
	
	coverageArea = 100 * coverageArea/(WORLDX * WORLDY);	
	return YES;
}


// draws the green line grid in the XY plane
- (void) DrawGrid
{
	GLint i;
	
	for(i = -100.0; i <= 100.0; i += 5.0)
	{
		if(i == 0) glLineWidth(4.0);
		else glLineWidth(1.0);
		glBegin(GL_LINES);
		glNormal3f(0.0,0.0,1.0);
		glColor3ub(50,50,50);						
		glVertex3f(-100, i, -0.01);					
		glVertex3f(100, i, -0.01);
		glVertex3f(i, -100, -0.01);							
		glVertex3f(i, 100, -0.01);
		glEnd();
	}
}

// draw the triangular spinning goal
- (void) GlobalGoal
{

	glPushMatrix();
	glTranslatef(goal.x,goal.y,goal.z);
	glRotatef(wind, 0.0, 0.0, 1.0);
	wind += .1;
	glBegin(GL_TRIANGLES);
				glColor4f(GREEN);
				glNormal3f(0.0,-1.0,0.0);
				glVertex3f(-GOALSIZE,-GOALSIZE,0.0);
				glVertex3f(GOALSIZE,-GOALSIZE,0.0);
				glVertex3f(0.0,0.0,GOALSIZE*GOALHEIGHT);
				glNormal3f(0.0,1.0,0.0);
				glVertex3f(GOALSIZE,GOALSIZE,0.0);
				glVertex3f(-GOALSIZE,GOALSIZE,0.0);
				glVertex3f(0.0,0.0,GOALSIZE*GOALHEIGHT);
				glColor4f(ICE);
				glNormal3f(1.0,0.0,0.0);
				glVertex3f(GOALSIZE,-GOALSIZE,0.0);
				glVertex3f(GOALSIZE,GOALSIZE,0.0);
				glVertex3f(0.0,0.0,GOALSIZE*GOALHEIGHT);
				glNormal3f(-1.0,0.0,0.0);
				glVertex3f(-GOALSIZE,GOALSIZE,0.0);
				glVertex3f(-GOALSIZE,-GOALSIZE,0.0);
				glVertex3f(0.0,0.0,GOALSIZE*GOALHEIGHT);
	glEnd();
	glPopMatrix();
}

// draw the obstacles and the Cspace on the screen if the arguments are YES
- (void)DrawWorldObstacles:(BOOL)obst Cspace:(BOOL)cspace
{
	int i;

	if(obst){
		for(i = 0; i < [obstacles count]; i++)
		{
			[[obstacles objectAtIndex:i] drawCrate];
		}
	}

	if(cspace){
		for(i = 0; i < [configSpace count]; i++)
		{
			[[configSpace objectAtIndex:i] drawCspacePoint:YES RadLine:NO];
		}
	}
}

// recalculates a new set of obstacles and Cspace
- (void)ResetObstacles
{			
	[obstacles release];
	[configSpace release];
	if(![self RandomizeObstacles])[self ResetObstacles];
	else worldNumber++;
}
	
// merge two Cspace objects
- (int) MergeCspace:(Cspace *)CS1 withCspace:(Cspace *)CS2
{
	int i,iplus,m;
	int j,jplus,n;
	int a,swap,k,l,v=0;
	int obj;
	float p,q;
	double z;
	BOOL inside;
	Point3f start;
	Point3f currentPt;
	Point3f newpoint;
	Point3f addpoint;
	Point3 *tempPoint;
	NSMutableArray *CSA1,*CSA2,*CSNew;

	if(magnitude3d([CS1 getCenter],[CS2 getCenter]) > ([CS1 getRadius] + [CS2 getRadius])) return 0;
	
	i = 0;
	j = 0;
	obj = 1;
	
	CSA1 = [CS1 getVertexArray];
	CSA2 = [CS2 getVertexArray];
	
	// test if the first point in object 1 is inside of object 2
	inside = [CS2 isPointInside:[[CSA1 objectAtIndex:0] getPtrPoint]];

	while(i <  [CSA1 count])
	{
		if(i == [CSA1 count] - 1) iplus = 0;
		else iplus = i+1;
		
		while(j < [CSA2 count])
		{
			if(j == [CSA2 count] - 1) jplus = 0;
			else jplus = j+1;
			
			// check for intersection of line segments
			if((v = SegmentIntersect([[CSA1 objectAtIndex:i] getPtrPoint],
									[[CSA1 objectAtIndex:iplus] getPtrPoint],
									[[CSA2 objectAtIndex:j] getPtrPoint],
									[[CSA2 objectAtIndex:jplus] getPtrPoint],
									&newpoint)))
			{
				if(inside)
				{
					k=0;
					start = [[CSA2 objectAtIndex:j] getPoint];
					addpoint = [[CSA2 objectAtIndex:jplus] getPoint];
					obj = 2;
				}
				else
				{
					k=0;
					start = [[CSA1 objectAtIndex:i] getPoint];
					addpoint = [[CSA1 objectAtIndex:iplus] getPoint];
				}
				break;
			}
			else j++;
		}
		
		if(v) break;
		if(j == [CSA2 count]) j=0;
		i++;
	}
	// after initial check if all the vertices in object 1 are checked
	// the three following cases are returned, otherwise we continue with the merging process
	if(i == [CSA1 count])
	{
		if(inside) return 1; // returns 1 if object 1 is inside object 2
		else if([CS1 isPointInside:[CS2 getCenter]]) return 2; // returns 2 if object 2 is inside object 1
		else return 0; // objects are near but do not intersect
	}

	// create a new array to hold the new cspace vertices
	CSNew = [[NSMutableArray alloc] init];
	tempPoint = [[Point3 alloc] initPoint3f:start];
	// place the starting point from above into the new array
	[CSNew addObject:tempPoint];
	[tempPoint release];
	
	a=0;
	l=0;
	
	// set the current point location to the current vertex we are operating on
	currentPt = [[CSNew objectAtIndex:a] getPoint];
	
	while(l == 0)
	{
	
	// do the following testing around the objects while the current point is not near the start point
	// if the current point is near the start point we have traversed around the entire object
	do{
	
		if(obj == 1)// if we start on object 1 perform
		{
			if(j == [CSA2 count] -1) jplus = 0;
			else jplus = j+1;
		
			// find an intersection point
			v = SegmentIntersect(&currentPt,
								 [[CSA1 objectAtIndex:iplus] getPtrPoint],
								 [[CSA2 objectAtIndex:j] getPtrPoint],
								 [[CSA2 objectAtIndex:jplus] getPtrPoint],
								 &newpoint);
		
			// check to make sure the newpoint and current point are not the same
			if(newpoint.x != currentPt.x && newpoint.y != currentPt.y)
			{
				// check if the newpoint is closer
				if(magnitude3d(&currentPt,&newpoint) < magnitude3d(&currentPt,&addpoint))
				{
				// z is positive if the vertex on object 2 is clockwise from the current point - intersect point
				// this is needed to check which line is being intersected on object 2
				// if z is negative we are on the far side of obstacle 2
					z = (([[CSA2 objectAtIndex:jplus] getPoint].x - currentPt.x)*
						 ([[CSA1 objectAtIndex:iplus] getPoint].y - currentPt.y))-
						(([[CSA2 objectAtIndex:jplus] getPoint].y - currentPt.y)*
						 ([[CSA1 objectAtIndex:iplus] getPoint].x - currentPt.x));
				
					if(v)
					{
					// state whether we have to swap to object 2
						if(z > 0)swap = 1;
						else swap = 0;	// otherwise stay on current object
						m = j;
						n = jplus;
					}
					addpoint = newpoint;
				}
			}
		
			// increment to next vertex on object 2 
			j++;
			// if all vertices on object 2 have been checked for the nearest intersection
			if(j == [CSA2 count])
			{
				a++;
				tempPoint = [[Point3 alloc] initPoint3f:addpoint];
				[CSNew addObject:tempPoint];
				// set the current point location to the current vertex we are operating on
				currentPt = [[CSNew objectAtIndex:a] getPoint];
				[tempPoint release];
				if(swap)
				{
					obj = 2;
					i = 0;
					j = m;
					jplus = n;
					swap = 0;
					addpoint = [[CSA2 objectAtIndex:jplus] getPoint];
				}
				else
				{
					j = 0;
					iplus++;
					if(iplus == [CSA1 count]) iplus = 0;
					addpoint = [[CSA1 objectAtIndex:iplus] getPoint];
				}
				l++;
			}
		}
		else
		{
			if(i == [CSA1 count] -1) iplus = 0;
			else iplus = i+1;
		
			// find an intersection point
			v = SegmentIntersect(&currentPt,
							 [[CSA2 objectAtIndex:jplus] getPtrPoint],
							 [[CSA1 objectAtIndex:i] getPtrPoint],
							 [[CSA1 objectAtIndex:iplus] getPtrPoint],
							 &newpoint);
			// check to make sure the newpoint and current point are not the same
			if(newpoint.x != currentPt.x && newpoint.y != currentPt.y)
			{
				// check if the newpoint is closer
				if(magnitude3d(&currentPt,&newpoint) < magnitude3d(&currentPt,&addpoint))
				{
				// z is positive if the vertex on object 2 is clockwise from the current point - intersect point
				// this is needed to check which line is being intersected on object 2
				// if z is negative we are on the far side of obstacle 2
					z = (([[CSA1 objectAtIndex:iplus] getPoint].x - currentPt.x)*
						 ([[CSA2 objectAtIndex:jplus] getPoint].y - currentPt.y))-
						(([[CSA1 objectAtIndex:iplus] getPoint].y - currentPt.y)*
						([[CSA2 objectAtIndex:jplus] getPoint].x - currentPt.x));
					if(v)
					{
						if(z > 0) swap = 1;
						else swap = 0;
						m = i;
						n = iplus;
					}
					addpoint = newpoint;
				}
			}
		
			// increment to next vertex on object 1
			i++;
				// if all object 1 vertices have been checked add the intersection or new point
			if(i == [CSA1 count])
			{
				a++;
				tempPoint = [[Point3 alloc] initPoint3f:addpoint];
				[CSNew addObject:tempPoint];
				// set the current point location to the current vertex we are operating on
				currentPt = [[CSNew objectAtIndex:a] getPoint];
				[tempPoint release];
			
				if(swap)
				{
					obj = 1;
					j = 0;
					i = m;
					iplus = n;
					swap = 0;
					addpoint = [[CSA1 objectAtIndex:iplus] getPoint];
				}
				else
				{
					i = 0;
					jplus++;
					if(jplus == [CSA2 count]) jplus = 0;
					addpoint = [[CSA2 objectAtIndex:jplus] getPoint];
				}
				l++;
			}
		}
	
		if(l > 100) 
		{
			//NSLog(@"merge stuck in loop");
			break;
		}
	}
	while(((fabs(currentPt.x - start.x) > 0.0005) || (fabs(currentPt.y - start.y) > 0.0005)) || l == 0);

		//////////////////////////////////////////////////////////////////////
		// checking for some situation where I missed something
		// It should only reach this point if it knows that the two objects intersect
		// Here it should find a point on object 1 that is outside object 2 then restart the merge
		if(l >= 100 || l <= 5)
		{

			//NSLog(@"problems in merging objects");
			[CSNew release];
			return -1;
		}
		else break;
		////////////////////////////////////////////////////////////////////		
	}
	
/////////////////////////////////////////////////////////////////
	[CSNew removeLastObject];
	// calculates a new average center position for the new Cspace object
	p = 0;
	q = 0;
	for(i=0; i < [CSNew count]; i++)
	{
		p += [[CSNew objectAtIndex:i] getPoint].x;
		q += [[CSNew objectAtIndex:i] getPoint].y;		
	}
	
	addpoint.x = p/[CSNew count];
	addpoint.y = q/[CSNew count];
	addpoint.z = 0;
	
	[CS1 setCenter:addpoint];
////////////////////////////////////////////////////////////////
	// calculates a new radius for the new Cspace object
	z = 0;
	for(i=0; i < [CSNew count]; i++)
	{
		z = magnitude3d([[CSNew objectAtIndex:i] getPtrPoint],&addpoint);
		if(z > [CS1 getRadius]){
			[CS1 setRadius:z];
			[CS1 setRadPoint:[[CSNew objectAtIndex:i] getPoint]];
		}
	}
///////////////////////////////////////////////////////////////
	// all checking is done the new VertexArray has been created and is in CSNew
	// replace the VertexArray in object 1 with the new array
	[CS1 setVertexArray:CSNew];
	
	[CSNew release];
	
	return 2; // remove object 2 after this function returns
}

// Cspace manegment function, adds and removes merged Cspace objects
- (int)CreateCspace
{
	int i,j;
	int state;
	
	i = 0;
	j = i+1;
	
	while(i < [configSpace count] - 1)
	{
		state = [self MergeCspace:[configSpace objectAtIndex:i] withCspace:[configSpace objectAtIndex:j]];
						
		if(state == 1)
		{
			[configSpace removeObjectAtIndex:i];
			j = i+1;
		}
		else if(state == 2)[configSpace removeObjectAtIndex:j];
		else if(state == -1) return -1;
		else j++;
	
		if(j >= [configSpace count])
		{
			i++;
			j = i+1;
		}
	}	

	for(i = 0; i < [configSpace count]; i++)
	{
		[[configSpace objectAtIndex:i] setObjectNum:i];
	}
	
	return 0;
}

- (double) getCoverageArea
{
	return coverageArea;
}

- (int) getNumberOfCspaceObjs
{
	return [configSpace count];
}

- (NSMutableArray *)getConfigurationArray
{
	return configSpace;
}

- (int) getWorldNum
{
	return worldNumber;
}

- (void) WorldCountReset
{
	worldNumber = 0;
}

- (int) numObstacles
{
	return numObstacles;
}

- (void) setNumObstacles:(int)n
{
	numObstacles = n;
}

@end
